/**
  ******************************************************************************
  * File Name          : motor_device.c
  * Description        : 电机实例相关，比如定义电机类型变量，存放各个电机
	*											 的初始化函数等。由用户自行编写
  ******************************************************************************
  */

#include "motor_device.h"

#include "motor.h"
#include "motor_bsp_stm32.h"
#include "main.h"
#include "can.h"
#include "tim.h"
#include "gpio.h"
#include "leg.h"
#include "usr_conf.h"

void Motor_Device_Base_Init(Motor_t* m, 
							uint8_t can_id, 
							CAN_HandleTypeDef* hcan,
							float angle_offset,
							int16_t init_current )
{
	/* 板级初始化 */
	m->bsp.can_id = can_id;
	m->bsp.hcan = hcan;

	m->bsp.abs_angle_offset = angle_offset;

	Motor_SetCtrlMode(m, MOTOR_CTRL_POS);
	
	/* 可能将来会加入变种PID，就在这里更改函数指针 */
	m->spd_pid.Calc = PID_Calc;
	m->pos_pid.Calc = PID_Calc;

	/* 自动校正，设置单向转 */
	Motor_SetPower(&(m->bsp), init_current);
}

void Motor_Device_PIDParam_Init(Motor_t* m,
								float spd_kp,float spd_ki,float spd_kd,
								float pos_kp, float pos_ki, float pos_kd, float pos_sumMax)
{
	/* PID 参数初始化 */
	m->spd_pid.kp = spd_kp;
	m->spd_pid.ki = spd_ki;
	m->spd_pid.kd = spd_kd;
	m->pos_pid.kp = pos_kp;
	m->pos_pid.ki = pos_ki;
	m->pos_pid.kd = pos_kd;
	m->pos_pid.sumMax = pos_sumMax;

	/* 各环输出限幅均设为0 */
	m->spd_pid.outputMax = 0;
	m->pos_pid.outputMax = 0;
}

void Motor_Offset_Init()
{
	for (int leg_index = 0; leg_index < 4; leg_index++)
	{
		leg[leg_index].m1.bsp.turns = 0;
		leg[leg_index].m2.bsp.turns = 0;
		
		leg[leg_index].m1.bsp.abs_angle_offset += leg[leg_index].m1.bsp.angle/819.2f/2.f;
		leg[leg_index].m2.bsp.abs_angle_offset += leg[leg_index].m2.bsp.angle/819.2f/2.f;
		
		leg[leg_index].m1.bsp.encoder_angle_offset = leg[leg_index].m1.bsp.angle;//RB 
		leg[leg_index].m2.bsp.encoder_angle_offset = leg[leg_index].m2.bsp.angle;
		
		leg[leg_index].m1.pos_pid.ref = 0;//RB 
		leg[leg_index].m2.pos_pid.ref = 0;
	}
//	rf_m1.bsp.abs_angle_offset += 2*(rf_m1.bsp.angle/819.2f/2.f);
//	rf_m1.pos_pid.ref = rf_m1.bsp.angle/819.2f/2.f;
}

void Motor_Device_InitAll()
{
	//#define MOTOR_ZERO_POS 43.27f
	#define MOTOR_ZERO_POS 24.74f
	/* 基本参数初始化 */
	#ifdef RB_ENABLE
		Motor_Device_Base_Init(&rb_m2, 2, &hcan1, MOTOR_ZERO_POS-180.f, -700);
		Motor_Device_Base_Init(&rb_m1, 1, &hcan1, -MOTOR_ZERO_POS-180.f, 700);
	#endif
	#ifdef LB_ENABLE
		Motor_Device_Base_Init(&lb_m1, 4, &hcan1,  MOTOR_ZERO_POS+180.f, -800);
		Motor_Device_Base_Init(&lb_m2, 3, &hcan1, -MOTOR_ZERO_POS+180.f, 700);
	#endif
	#ifdef RF_ENABLE
		Motor_Device_Base_Init(&rf_m1, 5, &hcan1, MOTOR_ZERO_POS+180.f, -700);
		Motor_Device_Base_Init(&rf_m2, 6, &hcan1, -MOTOR_ZERO_POS+180.f, +700);
	#endif
	#ifdef LF_ENABLE
		Motor_Device_Base_Init(&lf_m1, 7, &hcan1, -MOTOR_ZERO_POS-180.f, 700);
		Motor_Device_Base_Init(&lf_m2, 8, &hcan1, MOTOR_ZERO_POS-180.f, -700);
	#endif

	/* PID参数初始化 */
	Motor_Device_PIDParam_Init(&lf_m1, 5, 0, 0, 0.2, 0.001, 0, 10000);
	Motor_Device_PIDParam_Init(&lf_m2, 5, 0, 0, 0.2, 0.001, 0, 10000);
	Motor_Device_PIDParam_Init(&rf_m1, 5, 0, 0, 0.2, 0.001, 0, 10000);
	Motor_Device_PIDParam_Init(&rf_m2, 5, 0, 0, 0.2, 0.001, 0, 10000);
	Motor_Device_PIDParam_Init(&lb_m1, 5, 0, 0, 0.2, 0.001, 0, 10000);
	Motor_Device_PIDParam_Init(&lb_m2, 5, 0, 0, 0.2, 0.001, 0, 10000);
	Motor_Device_PIDParam_Init(&rb_m1, 5, 0, 0, 0.2, 0.001, 0, 10000);
	Motor_Device_PIDParam_Init(&rb_m2, 5, 0, 0, 0.2, 0.001, 0, 10000);

	/* 发送回零指令，即给一定的恒定电流，并等待300ms */
	#ifdef MOTOR_INIT_ENABLE
	Motor_Cmd_Tx();
	HAL_Delay(300);//此处使用HAL_Delay是因为此时还未进入操作系统，不是在线程中等待
	#endif

	/* 发送停止指令，完成校正 */
	for (int leg_index = 0; leg_index < 4; leg_index++)
	{
		Motor_SetPower(&(leg[leg_index].m1.bsp), 0);
		Motor_SetPower(&(leg[leg_index].m2.bsp), 0);
	}
	Motor_Cmd_Tx();
	HAL_Delay(1000);//此处使用HAL_Delay是因为此时还未进入操作系统，不是在线程中等待
	Motor_Offset_Init();

}
